// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: Support the reader to filter again through the writer's filtering results
// feature: E2E protection
// ------------------------------------------------------------------

#include <ertps/qos/ParameterTypes.h>

#include <edds/dds/core/policy/ParameterSerializer.hpp>
#include <edds/dds/core/policy/ParameterList.hpp>
#include <ertps/types/TypesBase.h>

namespace evbs {
namespace ertps {
namespace rtps {

using vbs::common::ChangeKind_t::ALIVE;
using vbs::common::ChangeKind_t::NOT_ALIVE_DISPOSED;
using vbs::common::ChangeKind_t::NOT_ALIVE_DISPOSED_UNREGISTERED;
using vbs::common::ChangeKind_t::NOT_ALIVE_UNREGISTERED;
namespace {

struct DataMsgUtils {
    static void prepare_submessage_flags(const CacheChange_t* change, TopicKind_t topicKind, bool& dataFlag,
                                         bool& keyFlag, bool& inlineQosFlag, octet& status,
                                         InlineQosWriter* inlineQos) {
        inlineQosFlag = (inlineQos != nullptr) || ((WITH_KEY == topicKind) && (change->kind != ALIVE)) ||
                        (change->write_params.related_sample_identity() != SampleIdentity::unknown());

        dataFlag = ((ALIVE == change->kind) && (change->serializedPayload.length > 0) &&
                    (nullptr != change->serializedPayload.data));
        keyFlag = (!dataFlag) && (!inlineQosFlag) && (WITH_KEY == topicKind);

        status = 0;
        if (change->kind == NOT_ALIVE_DISPOSED) {
            status = status | BIT(0);
        }
        if (change->kind == NOT_ALIVE_UNREGISTERED) {
            status = status | BIT(1);
        }
        if (change->kind == NOT_ALIVE_DISPOSED_UNREGISTERED) {
            status = status | BIT(0);
            status = status | BIT(1);
        }
    }

    static bool serialize_header(uint32_t fragment_number, CDRMessage_t* msg, const CacheChange_t* change,
                                 const EntityId_t& readerId, octet flags, uint32_t& submessage_size_pos,
                                 uint32_t& position_size_count_size) {
        bool ok = true;
        bool is_fragment = fragment_number > 0U;

        (void)CDRMessage::addOctet(msg, is_fragment ? DATA_FRAG : DATA);
        (void)CDRMessage::addOctet(msg, flags);
        submessage_size_pos = msg->pos;
        (void)CDRMessage::addUInt16(msg, 0U);
        position_size_count_size = msg->pos;

        // extra flags. not in this version.
        ok &= CDRMessage::addUInt16(msg, 0U);

        // octet to inline Qos is 12 or 28, may change in future versions
        ok &= is_fragment ? CDRMessage::addUInt16(msg, RTPSMESSAGE_OCTETSTOINLINEQOS_DATAFRAGSUBMSG)
                          : CDRMessage::addUInt16(msg, RTPSMESSAGE_OCTETSTOINLINEQOS_DATASUBMSG);
        // Entity ids
        ok &= CDRMessage::addEntityId(msg, &readerId);
        ok &= CDRMessage::addEntityId(msg, &change->writerGUID.entityId);
        // Add Sequence Number
        ok &= CDRMessage::addSequenceNumber(msg, &change->sequenceNumber);

        if (is_fragment) {
            // Add fragment starting number
            ok &= CDRMessage::addUInt32(msg, fragment_number);  // fragments start in 1

            // Add fragments in submessage
            ok &= CDRMessage::addUInt16(msg, 1U);  // we are sending one fragment

            // Add fragment size
            ok &= CDRMessage::addUInt16(msg, change->getFragmentSize());

            // Add total sample size
            ok &= CDRMessage::addUInt32(msg, (change->serializedPayload.length + change->nonSerializedPayload.length));
        }

        return ok;
    }

#ifdef BATCH_SEND_ENABLE
    static bool serialize_batch_header(CDRMessage_t* msg, const CacheChange_t* change, const EntityId_t& readerId,
                                       octet flags, uint32_t& submessage_size_pos, uint32_t& position_size_count_size) {
        bool ok = true;

        (void)CDRMessage::addOctet(msg, DATA_BATCH);
        (void)CDRMessage::addOctet(msg, flags);
        submessage_size_pos = msg->pos;
        (void)CDRMessage::addUInt16(msg, 0U);
        position_size_count_size = msg->pos;

        // extra flags. not in this version.
        ok &= CDRMessage::addUInt16(msg, 0U);
        // octet to inline Qos is 12 or 28, may change in future versions
        ok &= CDRMessage::addUInt16(msg, RTPSMESSAGE_OCTETSTOINLINEQOS_DATABATCHMSG);
        // Entity ids
        ok &= CDRMessage::addEntityId(msg, &readerId);
        ok &= CDRMessage::addEntityId(msg, &change->writerGUID.entityId);
        // Add Batch Sequence Number
        ok &= CDRMessage::addSequenceNumber(msg, &change->batchSequenceNumber);
        // Add firstSampleSeqNumber
        ok &= CDRMessage::addSequenceNumber(msg, &change->sequenceNumber);

        ok &= CDRMessage::addUInt32(msg, change->batchSampleCount - 1U);
        ok &= CDRMessage::addUInt32(msg, change->batchSampleCount);
        ok &= CDRMessage::addUInt32(msg, change->batchSampleCount << 3);
        return ok;
    }
#endif

    static void serialize_inline_qos(CDRMessage_t* msg, const CacheChange_t* change, TopicKind_t topicKind,
                                     octet status, InlineQosWriter* inlineQos) {
        if (change->write_params.related_sample_identity() != SampleIdentity::unknown()) {
            (void)edds::dds::ParameterSerializer<Parameter_t>::add_parameter_sample_identity(
                msg, change->write_params.related_sample_identity());
        }

        if ((WITH_KEY == topicKind) && (ALIVE != change->kind)) {
            (void)edds::dds::ParameterSerializer<Parameter_t>::add_parameter_key(msg, change->instanceHandle);

            if (ALIVE != change->kind) {
                (void)edds::dds::ParameterSerializer<Parameter_t>::add_parameter_status(msg, status);
            }
        }

        if (inlineQos != nullptr) {
            inlineQos->writeQosToCDRMessage(msg);
        }
        (void)edds::dds::ParameterSerializer<Parameter_t>::add_parameter_sentinel(msg);
    }
};

}  // namespace
bool RTPSMessageCreator::addSubmessageDataSingle(CDRMessage_t* msg, const CacheChange_t* change, TopicKind_t topicKind,
                                                 const EntityId_t& readerId, InlineQosWriter* inlineQos,
                                                 bool* is_big_submessage) {
    octet status = 0U;
    octet flags = 0U;
    // Find out flags
    bool dataFlag = false;
    bool keyFlag = false;
    bool inlineQosFlag = false;

    Endianness_t old_endianess = msg->msg_endian;
#if EDDS_IS_BIG_ENDIAN_TARGET
    msg->msg_endian = BIGEND;
#else
    flags = flags | BIT(0);
    msg->msg_endian = LITTLEEND;
#endif  // if EDDS_IS_BIG_ENDIAN_TARGET

    DataMsgUtils::prepare_submessage_flags(change, topicKind, dataFlag, keyFlag, inlineQosFlag, status, inlineQos);

    if (inlineQosFlag) {
        flags = flags | BIT(1);
    }

    if (dataFlag) {
        flags = flags | BIT(2);
    }

    if (keyFlag) {
        flags = flags | BIT(3);
    }

    // Submessage header.
    uint32_t submessage_size_pos = 0U;
    uint32_t position_size_count_size = 0U;
    bool added_no_error =
        DataMsgUtils::serialize_header(0U, msg, change, readerId, flags, submessage_size_pos, position_size_count_size);

    // Add INLINE QOS AND SERIALIZED PAYLOAD DEPENDING ON FLAGS:
    if (inlineQosFlag)  // inlineQoS
    {
        DataMsgUtils::serialize_inline_qos(msg, change, topicKind, status, inlineQos);
    }

    // Add Serialized Payload
    if (dataFlag) {
        const_cast<vbs::common::CacheChange_t*>(change)->serialized_data_offset = msg->pos;
        if (change->serializedPayload.origin_data) {
            added_no_error &= CDRMessage::addData(msg, change->serializedPayload.data,
                                                  change->serializedPayload.representation_header_size);
            added_no_error &= CDRMessage::addData(
                msg, change->serializedPayload.origin_data,
                change->serializedPayload.length - change->serializedPayload.representation_header_size);
        } else {
            added_no_error &=
                CDRMessage::addData(msg, change->serializedPayload.data, change->serializedPayload.length);
            if (change->nonSerializedPayload.length > 0) {
                added_no_error &=
                    CDRMessage::addData(msg, change->nonSerializedPayload.data, change->nonSerializedPayload.length);
            }
        }
    }

    if (keyFlag) {
        added_no_error &= CDRMessage::addOctet(msg, 0U);  // ENCAPSULATION
        if (msg->msg_endian == BIGEND) {
            added_no_error &= CDRMessage::addOctet(msg, PL_CDR_BE);  // ENCAPSULATION
        } else {
            added_no_error &= CDRMessage::addOctet(msg, PL_CDR_LE);  // ENCAPSULATION
        }
        added_no_error &= CDRMessage::addUInt16(msg, 0U);  // ENCAPSULATION OPTIONS
        added_no_error &= edds::dds::ParameterSerializer<Parameter_t>::add_parameter_key(msg, change->instanceHandle);
        added_no_error &= edds::dds::ParameterSerializer<Parameter_t>::add_parameter_status(msg, status);
        added_no_error &= edds::dds::ParameterSerializer<Parameter_t>::add_parameter_sentinel(msg);
    }

    // Align submessage to rtps alignment (4).
    uint32_t align = (4U - msg->pos % 4U) & 3U;
    for (uint32_t count = 0U; count < align; ++count) {
        added_no_error &= CDRMessage::addOctet(msg, 0U);
    }

    // if(align > 0)
    {
        // submsgElem.pos += align;
        // submsgElem.length += align;
    }

    uint32_t size32 = msg->pos - position_size_count_size;
    if (size32 <= std::numeric_limits<uint16_t>::max()) {
        uint16_t submessage_size = static_cast<uint16_t>(size32);
        octet* o = reinterpret_cast<octet*>(&submessage_size);

        if (msg->buffer == nullptr || submessage_size_pos + 1 >= msg->max_size) {
            return false;
        }

        if (msg->msg_endian == DEFAULT_ENDIAN) {
            msg->buffer[submessage_size_pos] = *(o);
            msg->buffer[submessage_size_pos + 1U] = *(o + 1);
        } else {
            msg->buffer[submessage_size_pos] = *(o + 1);
            msg->buffer[submessage_size_pos + 1U] = *(o);
        }

        *is_big_submessage = false;
    } else {
        // Submessage > 64KB
        *is_big_submessage = true;
    }

    msg->msg_endian = old_endianess;

    return added_no_error;
}

bool RTPSMessageCreator::addSubmessageDataBatch(CDRMessage_t* msg, const CacheChange_t* change, TopicKind_t topicKind,
                                                const EntityId_t& readerId, InlineQosWriter* inlineQos,
                                                bool* is_big_submessage) {
#ifdef BATCH_SEND_ENABLE
    octet status = 0U;
    octet flags = 0U;
    // Find out flags
    bool dataFlag = false;
    bool keyFlag = false;
    bool inlineQosFlag = false;

    Endianness_t old_endianess = msg->msg_endian;
#if EDDS_IS_BIG_ENDIAN_TARGET
    msg->msg_endian = BIGEND;
#else
    flags = flags | BIT(0);
    msg->msg_endian = LITTLEEND;
#endif  // if EDDS_IS_BIG_ENDIAN_TARGET

    DataMsgUtils::prepare_submessage_flags(change, topicKind, dataFlag, keyFlag, inlineQosFlag, status, inlineQos);

    if (inlineQosFlag) {
        flags = flags | BIT(1);
    }

    if (keyFlag) {
        flags = flags | BIT(3);
        elogError(RTPS_WRITER, RetCode_t::RETCODE_UNSUPPORTED, "Batch send not support topic with key yet.");
        return false;
    }

    // Submessage header.
    uint32_t submessage_size_pos = 0U;
    uint32_t position_size_count_size = 0U;
    bool added_no_error = DataMsgUtils::serialize_batch_header(msg, change, readerId, flags, submessage_size_pos,
                                                               position_size_count_size);

    // Add INLINE QOS AND SERIALIZED PAYLOAD DEPENDING ON FLAGS:
    if (inlineQosFlag)  // inlineQoS
    {
        DataMsgUtils::serialize_inline_qos(msg, change, topicKind, status, inlineQos);
    }

    for (auto it = change->batch_changes.begin(); it != change->batch_changes.end(); it++) {
        added_no_error &= CDRMessage::addOctet(msg, 0U);
        added_no_error &= CDRMessage::addOctet(msg, 8U);
        added_no_error &= CDRMessage::addUInt16(msg, RTPSMESSAGE_SUBMESSAGEHEADER_SIZE);
        added_no_error &= CDRMessage::addUInt32(msg, (*it)->serializedPayload.length);
    }
    added_no_error &= CDRMessage::addOctet(msg, 0U);  // ENCAPSULATION
    if (msg->msg_endian == BIGEND) {
        added_no_error &= CDRMessage::addOctet(msg, CDR_BE);  // ENCAPSULATION
    } else {
        added_no_error &= CDRMessage::addOctet(msg, CDR_LE);  // ENCAPSULATION
    }
    added_no_error &= CDRMessage::addUInt16(msg, 0U);  // ENCAPSULATION OPTIONS

    for (auto it = change->batch_changes.begin(); it != change->batch_changes.end(); it++) {
        added_no_error &= CDRMessage::addData(msg, (*it)->serializedPayload.data, (*it)->serializedPayload.length);
    }

    // Align submessage to rtps alignment (4).
    uint32_t align = (4U - msg->pos % 4U) & 3U;
    for (uint32_t count = 0U; count < align; ++count) {
        added_no_error &= CDRMessage::addOctet(msg, 0U);
    }

    uint32_t size32 = msg->pos - position_size_count_size;
    if (size32 <= std::numeric_limits<uint16_t>::max()) {
        uint16_t submessage_size = static_cast<uint16_t>(size32);
        octet* o = reinterpret_cast<octet*>(&submessage_size);
        if (msg->msg_endian == DEFAULT_ENDIAN) {
            msg->buffer[submessage_size_pos] = *(o);
            msg->buffer[submessage_size_pos + 1U] = *(o + 1);
        } else {
            msg->buffer[submessage_size_pos] = *(o + 1);
            msg->buffer[submessage_size_pos + 1U] = *(o);
        }
        *is_big_submessage = false;
    } else {
        *is_big_submessage = true;
    }
    msg->msg_endian = old_endianess;

    return added_no_error;
#else
    (void)msg;
    (void)change;
    (void)topicKind;
    (void)readerId;
    (void)inlineQos;
    (void)is_big_submessage;
    return false;
#endif
}

bool RTPSMessageCreator::addSubmessageData(CDRMessage_t* msg, const CacheChange_t* change, TopicKind_t topicKind,
                                           const EntityId_t& readerId, InlineQosWriter* inlineQos,
                                           bool* is_big_submessage) {
    bool ret;
#ifdef BATCH_SEND_ENABLE
    if (!change->batch_send) {
        ret = addSubmessageDataSingle(msg, change, topicKind, readerId, inlineQos, is_big_submessage);
    } else {
        ret = addSubmessageDataBatch(msg, change, topicKind, readerId, inlineQos, is_big_submessage);
    }
#else
    ret = addSubmessageDataSingle(msg, change, topicKind, readerId, inlineQos, is_big_submessage);
#endif
    return ret;
}

bool RTPSMessageCreator::addSubmessageDataFrag(CDRMessage_t* msg, const CacheChange_t* change, uint32_t fragment_number,
                                               const CacheChange_t& change_to_add, TopicKind_t topicKind,
                                               const EntityId_t& readerId, InlineQosWriter* inlineQos) {
    octet status = 0U;
    octet flags = 0U;
    // Find out flags
    bool dataFlag = false;
    bool keyFlag = false;
    bool inlineQosFlag = false;

    Endianness_t old_endianess = msg->msg_endian;
#if EDDS_IS_BIG_ENDIAN_TARGET
    msg->msg_endian = BIGEND;
#else
    flags = flags | BIT(0);
    msg->msg_endian = LITTLEEND;
#endif  // if EDDS_IS_BIG_ENDIAN_TARGET

    DataMsgUtils::prepare_submessage_flags(change, topicKind, dataFlag, keyFlag, inlineQosFlag, status, inlineQos);

    if (inlineQosFlag) {
        flags = flags | BIT(1);
    }

    if (keyFlag) {
        flags = flags | BIT(2);
    }

    // Submessage header.
    uint32_t submessage_size_pos = 0U;
    uint16_t submessage_size = 0U;
    uint32_t position_size_count_size = 0U;
    bool added_no_error = DataMsgUtils::serialize_header(fragment_number, msg, change, readerId, flags,
                                                         submessage_size_pos, position_size_count_size);

    // Add INLINE QOS AND SERIALIZED PAYLOAD DEPENDING ON FLAGS:
    if (inlineQosFlag)  // inlineQoS
    {
        DataMsgUtils::serialize_inline_qos(msg, change, topicKind, status, inlineQos);
    }

    // Add Serialized Payload XXX TODO
    if (!keyFlag)  // keyflag = 0 means that the serializedPayload SubmessageElement contains the
                   // serialized Data
    {
        if (change->serializedPayload.origin_data) {
            if (fragment_number == 1) {
                added_no_error &= CDRMessage::addData(msg, change_to_add.serializedPayload.data,
                                                      change_to_add.serializedPayload.representation_header_size);
                added_no_error &= CDRMessage::addData(msg, change_to_add.serializedPayload.origin_data,
                                                      change_to_add.serializedPayload.length -
                                                          change_to_add.serializedPayload.representation_header_size);
            } else {
                added_no_error &= CDRMessage::addData(msg, change_to_add.serializedPayload.origin_data,
                                                      change_to_add.serializedPayload.length);
            }
        } else {
            added_no_error &=
                CDRMessage::addData(msg, change_to_add.serializedPayload.data, change_to_add.serializedPayload.length);
            if (change_to_add.nonSerializedPayload.length > 0) {
                added_no_error &= CDRMessage::addData(msg, change_to_add.nonSerializedPayload.data,
                                                      change_to_add.nonSerializedPayload.length);
            }
        }
    } else {
        // keyflag = 1 means that the serializedPayload SubmessageElement contains the serialized
        // Key
        /*
            added_no_error &= CDRMessage::addOctet(&submsgElem, 0); //ENCAPSULATION
            if (submsgElem.msg_endian == BIGEND)
            added_no_error &= CDRMessage::addOctet(&submsgElem, PL_CDR_BE); //ENCAPSULATION
            else
            added_no_error &= CDRMessage::addOctet(&submsgElem, PL_CDR_LE); //ENCAPSULATION

            added_no_error &= CDRMessage::addUInt16(&submsgElem, 0); //ENCAPSULATION OPTIONS
            added_no_error &= Parameter_t::addParameterKey(&submsgElem, &change->instanceHandle);
            added_no_error &= Parameter_t::addParameterStatus(&submsgElem, status);
            added_no_error &= Parameter_t::addParameterSentinel(&submsgElem);
         */
        msg->msg_endian = old_endianess;
        return false;
    }

    // TODO(Ricardo) This should be on cachechange.
    // Align submessage to rtps alignment (4).
    submessage_size = uint16_t(msg->pos - position_size_count_size);
    for (; submessage_size & 3U; ++submessage_size) {
        added_no_error &= CDRMessage::addOctet(msg, 0U);
    }

    // TODO(Ricardo) Improve.
    octet* o = reinterpret_cast<octet*>(&submessage_size);

    if (msg->buffer == nullptr || submessage_size_pos + 1 >= msg->max_size) {
        return false;
    }

    if (msg->msg_endian == DEFAULT_ENDIAN) {
        msg->buffer[submessage_size_pos] = *(o);
        msg->buffer[submessage_size_pos + 1U] = *(o + 1);
    } else {
        msg->buffer[submessage_size_pos] = *(o + 1);
        msg->buffer[submessage_size_pos + 1U] = *(o);
    }

    msg->msg_endian = old_endianess;

    return added_no_error;
}

}  // namespace rtps
}  // namespace ertps
}  // namespace evbs
